作動関節マニピュレータの逆運動学

Created by Masahiro Furukawa, Aug 18, 2020

#参考URL -> https://qiita.com/tibigame/items/61cecf86fc978628bfee
#参考図書 -> ポールのロボット・マニピュレータ

import numpy as np
import sympy as sym
sym.init_printing()
Pi = sym.S.Pi # 円周率
E = sym.S.Exp1 # 自然対数の底
I = sym.S.ImaginaryUnit # 虚数単位

#sympyの円周率の方を使うことをすすめる（こっちの方が量子化誤差が大きくなる．numpyも同様に大きい）
import math
pi = math.pi

# 角度変数
(J_1,J_2,J_3,J_4,J_5,J_6) = sym.symbols('J_1,J_2,J_3,J_4,J_5,J_6')

# リンクパラメータ
(a_1,a_2,a_3,d_4) = sym.symbols('a_1,a_2,a_3,d_4')

# リンクパラメータ
(j,a,d,alpha) = sym.symbols('j,a,d,alpha')

# T6
(n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z) = sym.symbols('n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z')

#sin.cosの簡易記述用
def S(a):
    return sym.sin(a)

def C(a):
    return sym.cos(a)

#回転・並進行列
def rotx(a):
    return sym.Matrix([[1, 0, 0, 0], [0, C(a), -S(a), 0], [0, S(a), C(a), 0], [0, 0, 0, 1]])

def roty(a):
    return sym.Matrix([[C(a), 0, S(a), 0], [0, 1, 0, 0], [-S(a), 0, C(a), 0], [0, 0, 0, 1]])

def rotz(a):
    return sym.Matrix([[C(a), -S(a), 0, 0], [S(a), C(a), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

def trans(x, y, z):
    return sym.Matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])

# DH matrix 
def DH(j, alpha, a, d):
    return rotz(j)*trans(a,0,d)*rotx(alpha)

# inverse DH matrix
def DHi(j, alpha, a, d):
    return rotx(-alpha)*trans(-a,0,-d)*rotz(-j)

DH法に基づく座標系間の関係表現

座標系 i	Z_i-1軸回りに角度θ_i	X_i軸周りにねじれ角α_iだけ回転	回転後のX_i-1 (=X_i)に沿って長さa_iだけ並進	Z_i-1に沿って距離d_iだけ並進
1	$J_1$	$\pi/2$	$a_1$	0
2	$J_2+\pi/2$	0	$a_2$	0
3	$J_3-J_2$	$\pi/2$	$a_3$	0
4	$J_4$	$-\pi/2$	0	$d_4$
5	$J_5$	$\pi/2$	0	0
6	$J_6$	0	0	0	$

変換行列 A

# A1=sym.trigsimp( DH (J_1,             Pi/2, a_1, 0))
# A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, a_3, 0))
A1=sym.trigsimp( DH (J_1,             Pi/2, 0, 0))
A2=sym.trigsimp( DH (J_2+ Pi/2,       0, a_2, 0))
A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, 0, 0))
A4=sym.trigsimp( DH (J_4,            -Pi/2,      0, d_4))
A5=sym.trigsimp( DH (J_5,             Pi/2,      0, 0))
A6=sym.trigsimp( DH (J_6,                   0,     0, 0))

# inverse matrix
# A1i=sym.trigsimp( DHi (J_1,             Pi/2, a_1, 0))
# A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, a_3, 0))
A1i=sym.trigsimp( DHi (J_1,             Pi/2, 0, 0))
A2i=sym.trigsimp( DHi (J_2+ Pi/2,       0, a_2, 0))
A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, 0, 0))
A4i=sym.trigsimp( DHi (J_4,            -Pi/2,      0, d_4))
A5i=sym.trigsimp( DHi (J_5,             Pi/2,      0, 0))
A6i=sym.trigsimp( DHi (J_6,                   0,     0, 0))

A3

$\displaystyle \left[\begin{matrix}\cos{\left(J_{2} - J_{3} \right)} & 0 & - \sin{\left(J_{2} - J_{3} \right)} & 0\\- \sin{\left(J_{2} - J_{3} \right)} & 0 & - \cos{\left(J_{2} - J_{3} \right)} & 0\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

# 逆行列をかけると単位行列になることの確認
ret = A2i*A2
sym.trigsimp(ret)

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

逆運動学

T6=sym.Matrix([[n_x, o_x, a_x, p_x], [n_y, o_y, a_y, p_y], [n_z, o_z, a_z, p_z], [0, 0, 0, 1]])
T6

$\displaystyle \left[\begin{matrix}n_{x} & o_{x} & a_{x} & p_{x}\\n_{y} & o_{y} & a_{y} & p_{y}\\n_{z} & o_{z} & a_{z} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]$

# forward kinematics
A56 = sym.trigsimp( A5*A6 )
A456 = sym.trigsimp( A4*A5*A6 )
A3456 = sym.trigsimp( A3*A4*A5*A6 )
A23456 = sym.trigsimp( A2*A3*A4*A5*A6 )
T  = sym.trigsimp( A1*A2*A3*A4*A5*A6 )

T

$\displaystyle \left[\begin{matrix}- \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} & \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & d_{4} \cos{\left(J_{3} \right)}\\- \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} - \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & d_{4} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

順運動学計算用Cソースコードを得る

# Masahiro Furukawa
# Aug, 17, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4

# Cソースを得る
for ii in range(4):
    for jj in range(4):
        idx = jj*4+ii
        code = sym.ccode(T[idx],assign_to=('Trans['+str(jj)+']['+str(ii)+']'), standard='C89')
        print(code)
    print()

Trans[0][0] = -(sin(J_3)*cos(J_4)*cos(J_5) + sin(J_5)*cos(J_3))*cos(J_6) + sin(J_3)*sin(J_4)*sin(J_6);
Trans[1][0] = -sin(J_4)*cos(J_5)*cos(J_6) - sin(J_6)*cos(J_4);
Trans[2][0] = (-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*cos(J_6) - sin(J_4)*sin(J_6)*cos(J_3);
Trans[3][0] = 0;
()
Trans[0][1] = (sin(J_3)*cos(J_4)*cos(J_5) + sin(J_5)*cos(J_3))*sin(J_6) + sin(J_3)*sin(J_4)*cos(J_6);
Trans[1][1] = sin(J_4)*sin(J_6)*cos(J_5) - cos(J_4)*cos(J_6);
Trans[2][1] = -(-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*sin(J_6) - sin(J_4)*cos(J_3)*cos(J_6);
Trans[3][1] = 0;
()
Trans[0][2] = -sin(J_3)*sin(J_5)*cos(J_4) + cos(J_3)*cos(J_5);
Trans[1][2] = -sin(J_4)*sin(J_5);
Trans[2][2] = sin(J_3)*cos(J_5) + sin(J_5)*cos(J_3)*cos(J_4);
Trans[3][2] = 0;
()
Trans[0][3] = -a_2*sin(J_2) + d_4*cos(J_3);
Trans[1][3] = 0;
Trans[2][3] = a_2*cos(J_2) + d_4*sin(J_3);
Trans[3][3] = 1;
()

inverse kinematics

$$ % reference : https://qiita.com/namoshika/items/63db972bfd1030f8264a % 空白は表示に影響しない。コメントは"%"で始める % 下付き文字は"_a"、上付き文字は"^a" % 改行は"\\"を付ける {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = \boldsymbol{A}_2 \boldsymbol{A}_3 \boldsymbol{A}_4 \boldsymbol{A}_5 \boldsymbol{A}_6 　　　(3.75)\\ {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = ^{1}\boldsymbol{T}_6 　　　(3.76) % 複数文字を1要素とする際は{...}で囲う % 空白は"\quad" $$

T16 = sym.trigsimp(                         A1i*T6 ) #  eq(3.70)
T26 = sym.trigsimp(                   A2i*A1i*T6 ) #  eq(3.71)
T36 = sym.trigsimp(             A3i*A2i*A1i*T6 ) #  eq(3.72)
T46 = sym.trigsimp(       A4i*A3i*A2i*A1i*T6 ) #  eq(3.73)
T56 = sym.trigsimp( A5i*A4i*A3i*A2i*A1i*T6 ) #  eq(3.74)

# Left hand  of (3.76)
A1iT6 = T16
A1iT6

$\displaystyle \left[\begin{matrix}n_{x} & o_{x} & a_{x} & p_{x}\\n_{z} & o_{z} & a_{z} & p_{z}\\- n_{y} & - o_{y} & - a_{y} & - p_{y}\\0 & 0 & 0 & 1\end{matrix}\right]$

# Right hand of (3.76)
A23456

$\displaystyle \left[\begin{matrix}- \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} & \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & d_{4} \cos{\left(J_{3} \right)}\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & d_{4} \sin{\left(J_{3} \right)}\\\sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

# eq2 (3.76)
sym.Eq(A1iT6[11] , A23456[11])

$\displaystyle - p_{y} = 0$

$$ \displaystyle \frac{ \sin{\left(J_{1} \right)} }{ \cos{\left(J_{1} \right)} } = \frac{p_{y}}{p_{x}} \\ \rightarrow \displaystyle \tan{\left({ J_{1} }\right)} = \frac{p_{y}}{p_{x}} \\ $$

Therefore $$\rightarrow \displaystyle J_{1} = atan2{\left({p_{y}},{p_{x}}\right)} \\ $$

# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(
    A1iT6[3]*A1iT6[3] + A1iT6[7]*A1iT6[7],
    A23456[3]* A23456[3] + A23456[7]*A23456[7]) ) )

$\displaystyle a_{2}^{2} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} d_{4} \sin{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + d_{4}^{2} = a_{1}^{2} - 2 a_{1} p_{x} \cos{\left(J_{1} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} + p_{x}^{2} \cos^{2}{\left(J_{1} \right)} + p_{x} p_{y} \sin{\left(2 J_{1} \right)} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} + p_{z}^{2}$

# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(A1iT6[7]*A1iT6[7] , A23456[7]*A23456[7]) ) )

$\displaystyle p_{z}^{2} = a_{2}^{2} \cos^{2}{\left(J_{2} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} d_{4} \sin{\left(J_{3} \right)} \cos{\left(J_{2} \right)} + a_{3}^{2} \cos^{2}{\left(J_{3} \right)} + a_{3} d_{4} \sin{\left(2 J_{3} \right)} + d_{4}^{2} \sin^{2}{\left(J_{3} \right)}$

T36

$\displaystyle \left[\begin{matrix}- n_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - n_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + n_{z} \cos{\left(J_{3} \right)} & - o_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + o_{z} \cos{\left(J_{3} \right)} & - a_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - a_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + a_{z} \cos{\left(J_{3} \right)} & a_{1} \sin{\left(J_{3} \right)} - a_{2} \cos{\left(J_{2} - J_{3} \right)} - a_{3} - p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + p_{z} \cos{\left(J_{3} \right)}\\n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} & o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} & a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)} & p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)}\\n_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + n_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + n_{z} \sin{\left(J_{3} \right)} & o_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + o_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + o_{z} \sin{\left(J_{3} \right)} & a_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{z} \sin{\left(J_{3} \right)} & - a_{1} \cos{\left(J_{3} \right)} + a_{2} \sin{\left(J_{2} - J_{3} \right)} + p_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{z} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

A456

$\displaystyle \left[\begin{matrix}- \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} & \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} & 0\\\sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\- \sin{\left(J_{5} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} & \cos{\left(J_{5} \right)} & d_{4}\\0 & 0 & 0 & 1\end{matrix}\right]$

# eq2 (3.76)
for idx in [3,11]:
    display(sym.simplify ( sym.expand( sym.Eq( T36[idx]*T36[idx], A456[idx]* A456[idx])) ) )

$\displaystyle a_{1}^{2} \sin^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{1} a_{3} \sin{\left(J_{3} \right)} - 2 a_{1} p_{x} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} + a_{1} p_{z} \sin{\left(2 J_{3} \right)} + a_{2}^{2} \cos^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + 2 a_{3} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{3} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} - 2 a_{3} p_{z} \cos{\left(J_{3} \right)} + p_{x}^{2} \sin^{2}{\left(J_{3} \right)} \cos^{2}{\left(J_{1} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - \frac{p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} - \frac{p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{z}^{2} \cos^{2}{\left(J_{3} \right)} = 0$

$\displaystyle d_{4}^{2} = a_{1}^{2} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{1} p_{x} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - a_{1} p_{z} \sin{\left(2 J_{3} \right)} + a_{2}^{2} \sin^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{2} - J_{3} \right)} + p_{x}^{2} \cos^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + \frac{p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + \frac{p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{z}^{2} \sin^{2}{\left(J_{3} \right)}$

# eq2 (3.76)
display(sym.trigsimp ( sym.expand( sym.Eq( T36[3]*T36[3] - T36[11]*T36[11], A456[3]* A456[3]-A456[11]* A456[11])) ) )

$\displaystyle a_{1}^{2} \sin^{2}{\left(J_{3} \right)} - a_{1}^{2} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{1} a_{2} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{1} a_{3} \sin{\left(J_{3} \right)} - 2 a_{1} p_{x} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{1} p_{x} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} + 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 a_{1} p_{z} \sin{\left(2 J_{3} \right)} - a_{2}^{2} \sin^{2}{\left(J_{2} - J_{3} \right)} + a_{2}^{2} \cos^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{x} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{2} p_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + 2 a_{3} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{3} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} - 2 a_{3} p_{z} \cos{\left(J_{3} \right)} + p_{x}^{2} \sin^{2}{\left(J_{3} \right)} \cos^{2}{\left(J_{1} \right)} - p_{x}^{2} \cos^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right) + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} - p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right) - p_{z}^{2} \sin^{2}{\left(J_{3} \right)} + p_{z}^{2} \cos^{2}{\left(J_{3} \right)} = - d_{4}^{2}$

# eq2 (3.76)
idx=6
sym.Eq(A456[idx] , T36[idx])

$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} = a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}$

sym.trigsimp(             A1*A2*A3*A4)

$\displaystyle \left[\begin{matrix}\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} & - \frac{\cos{\left(J_{1} - J_{3} \right)}}{2} - \frac{\cos{\left(J_{1} + J_{3} \right)}}{2} & \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{1} \right)}\\- \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} & - \frac{\sin{\left(J_{1} - J_{3} \right)}}{2} - \frac{\sin{\left(J_{1} + J_{3} \right)}}{2} & \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{1} \right)}\\\cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{3} \right)} & - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} & a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

sym.trigsimp(            A5i*A6i*T6)

$\displaystyle \left[\begin{matrix}a_{x} \sin{\left(J_{5} \right)} + \left(n_{x} \cos{\left(J_{6} \right)} - o_{x} \sin{\left(J_{6} \right)}\right) \cos{\left(J_{5} \right)} & - a_{x} \cos{\left(J_{5} \right)} + \left(n_{x} \cos{\left(J_{6} \right)} - o_{x} \sin{\left(J_{6} \right)}\right) \sin{\left(J_{5} \right)} & n_{x} \sin{\left(J_{6} \right)} + o_{x} \cos{\left(J_{6} \right)} & p_{x}\\a_{y} \sin{\left(J_{5} \right)} + \left(n_{y} \cos{\left(J_{6} \right)} - o_{y} \sin{\left(J_{6} \right)}\right) \cos{\left(J_{5} \right)} & - a_{y} \cos{\left(J_{5} \right)} + \left(n_{y} \cos{\left(J_{6} \right)} - o_{y} \sin{\left(J_{6} \right)}\right) \sin{\left(J_{5} \right)} & n_{y} \sin{\left(J_{6} \right)} + o_{y} \cos{\left(J_{6} \right)} & p_{y}\\a_{z} \sin{\left(J_{5} \right)} + \left(n_{z} \cos{\left(J_{6} \right)} - o_{z} \sin{\left(J_{6} \right)}\right) \cos{\left(J_{5} \right)} & - a_{z} \cos{\left(J_{5} \right)} + \left(n_{z} \cos{\left(J_{6} \right)} - o_{z} \sin{\left(J_{6} \right)}\right) \sin{\left(J_{5} \right)} & n_{z} \sin{\left(J_{6} \right)} + o_{z} \cos{\left(J_{6} \right)} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]$